module cfo_comp(
    input clk,
    input rst,
    input find,
    input find_sp,
    input strb,
    input [15:0] in_re,
    input [15:0] in_im,
    input [15:0] cfo,
    output [15:0] out_re,
    output [15:0] out_im,
    output out_strb,
    output [31:0] test_cordic_out,
    output      test_cordic_valid
);
    parameter size   = 12'd2048;
    parameter width  = 16;
    parameter idle   = 4'h0;
    parameter s0     = 4'h1;        //shift by 6 clks
    parameter s1     = 4'h2;        //modify cfo, start cordic
    parameter s2     = 4'h4;        //run cordic, get output
    parameter s3     = 4'h8;        //show output
    reg  [3:0] state;
    
    // strb_r
    reg strb_r;
    always @(posedge clk) begin
        if(rst) strb_r <= 1'b0;
        else strb_r <= strb;
    end
    
    // find_r
    reg find_r;
    always @(posedge clk) begin
        if (rst) find_r <= 1'b0;
        else find_r <= find & find_sp;
    end
    
    //shift seq
    parameter addr_width = $clog2(size);
    reg  [width-1:0] seq_re [0:size+6-1];
    reg  [width-1:0] seq_im [0:size+6-1];
    reg  [3:0]       sr_cnt;
    reg  [addr_width-1:0] seq_cnt;
    

    integer i;
    always @(posedge clk) begin
        if(rst) begin
            seq_re[0]   <= 'b0;
            seq_im[0]   <= 'b0;
        end
        else case(state)
            s0:begin
                if(strb_r) begin
                    seq_re[0] <= in_re;
                    seq_im[0] <= in_im;
                    for(i = size+6-1;i > 0;i = i-1) begin
                        seq_re[i] <= seq_re[i-1];
                        seq_im[i] <= seq_im[i-1];
                    end
                end
            end
            s1:begin
                seq_re[0] <= 'b0;
                seq_im[0] <= 'b0;
                for(i = size+6-1;i > 0;i = i-1) begin
                    seq_re[i] <= seq_re[i-1];
                    seq_im[i] <= seq_im[i-1];
                end
            end
            s2:begin
                if(seq_cnt > 'b0) begin
                    seq_re[0] <= 'b0;
                    seq_im[0] <= 'b0;
                    for(i = size+6-1;i > 0;i = i-1) begin
                        seq_re[i] <= seq_re[i-1];
                        seq_im[i] <= seq_im[i-1];
                    end
                end
            end
            s3:begin
                if(strb_r) begin
                    seq_re[0] <= in_re;
                    seq_im[0] <= in_im;
                    for(i = size+6-1;i > 0;i = i-1) begin
                        seq_re[i] <= seq_re[i-1];
                        seq_im[i] <= seq_im[i-1];
                    end
                end
            end
        endcase
    end

    always @(posedge clk) begin
        if(rst) begin
            sr_cnt <= 'b0;
        end
        else case(state)
            idle   : sr_cnt <= 'b0;
            default: sr_cnt <= (sr_cnt >= 4'd5) ? 'b0 : (sr_cnt + 1'b1);
        endcase
    end

    //cfo modified and run
    parameter               half_sc    = 21'b1_11_1111_1111_1110_0000_00; // scaled radian of -2pi/NFFT/2
    reg  [width-1+5:0]      cfo_md_r;
    reg  [width-1+5:0]      phase_add_r;
    reg  [width-1+5:0]      phase_r;
    reg  seq_last_r;
    
    wire seq_last_w;
    wire [width-1+5:0]      phase_w;
    
    assign                  phase_w    = phase_r;
    assign                  seq_last_w = seq_last_r;
  
      
    always @(posedge clk) begin
        if(rst) cfo_md_r      <= 'b0;
        else case(state)
            s1: cfo_md_r      <= {{12{cfo[width-1]}},cfo[width-2:width-2-(21-12)+1]};
        endcase
    end

    always @(posedge clk) begin
        if(rst) phase_add_r   <='b0;
        else case(state)
            s1: phase_add_r   <= {{12{cfo[width-1]}},cfo[width-2:width-2-(21-12)+1]};
            s2: phase_add_r   <= phase_add_r + cfo_md_r +  half_sc;
        endcase
    end

    always @(posedge clk) begin//-pi~pi
        if(rst) phase_r <= 'b0;
        else case(phase_add_r[ width-1+5: width-1+5-2])
            3'b001:phase_r    <= {~(phase_add_r [width-1+5 : width-1+5-1 ])  , phase_add_r [width-1+5-2 :0 ]};
            3'b010:phase_r    <= { 2'b00                                     , phase_add_r [width-1+5-2 :0 ]};
            3'b011:phase_r    <= {~(phase_add_r [width-1+5] )                , phase_add_r [width-1+5-1 :0 ]};
            3'b100:phase_r    <= {~(phase_add_r [width-1+5] )                , phase_add_r [width-1+5-1 :0 ]};
            3'b101:phase_r    <= { 2'b11                                     , phase_add_r [width-1+5-2 :0 ]};
            3'b110:phase_r    <= {~(phase_add_r [width-1+5 : width-1+5-1 ])  , phase_add_r [width-1+5-2 :0 ]};
            default: phase_r  <= phase_add_r;
        endcase
    end

    always @(posedge clk) begin
        if(rst) seq_cnt <= 'b0;
        else case(state)
            s1: seq_cnt       <= size - 3'd1;
            s2: seq_cnt       <= (seq_cnt == 'b0) ? 'b0 :(seq_cnt-1'b1);
        endcase
    end
    
    always @(posedge clk) begin
        if(rst) seq_last_r      <= 1'b0;
        else case(state)
            s2: seq_last_r      <= ((seq_cnt == 'b0) & (seq_last_r == 1'b0) )? 1'b1:1'b0;
            default: seq_last_r <= 1'b0;
        endcase
    end

    //cordic start and run
    parameter cordic_width = width + width + 5 + 3 + 5;
    reg  [cordic_width-1:0]     cordic_in_r;
    reg  [cordic_width-1:0]     cordic_in_r_r;
    reg                         cordic_en_r;
    reg                         cordic_en_r_r;

    wire [cordic_width-1:0]     cordic_in;
    wire                        cordic_en;
    wire                        cordic_fin;
    wire [width+width-1:0]      cordic_out; 
    wire cordic_out_last;
    assign cordic_en            = cordic_en_r_r;
    assign cordic_in            = cordic_in_r_r;
    assign test_cordic_out      = cordic_out;
    assign test_cordic_valid    = cordic_fin;
    
    cordic_comp comp_ins (.aclk(clk), .s_axis_phase_tvalid(cordic_en), 
      .s_axis_phase_tdata(phase_w), .s_axis_cartesian_tvalid(cordic_en), .s_axis_cartesian_tlast(seq_last_w),.s_axis_cartesian_tdata(cordic_in), 
      .m_axis_dout_tvalid(cordic_fin), .m_axis_dout_tlast(cordic_out_last),.m_axis_dout_tdata(cordic_out));
      
    always @(posedge clk) begin
        if(rst) begin
            cordic_in_r   <= 'b0;
            cordic_en_r   <= 1'b0;
        end
        else case(state)
            s1:begin
                cordic_in_r   <= {seq_im[size+6-1],5'b00000,3'bZZZ,seq_re[size+6-1],5'b00000};              
                cordic_en_r <= 1'b1;
            end
            s2:begin
                cordic_in_r   <= {seq_im[size+6-1],5'b00000,3'bZZZ,seq_re[size+6-1],5'b00000};
                cordic_en_r   <= (seq_cnt == 'b0 ) ? 1'b0 :1'b1;

            end
        endcase
   end

   always @(posedge clk) begin
        if(rst) begin
            cordic_en_r_r <= 1'b0;
            cordic_in_r_r <= 'b0;
        end
        else begin
            cordic_en_r_r <= cordic_en_r;
            cordic_in_r_r <= cordic_in_r;
        end
   end

    //fft
    parameter fft_width = width + width;
    wire [fft_width-1:0] fft_out;
    wire fft_fin;
    wire fft_last;

    fft_2048 fft_2048_ins(.aclk(clk), .s_axis_config_tdata(13'b01_11_11_10_01_01_1), //13'b01_10_11_10_10_01_1
  .s_axis_config_tvalid(1'b1), .s_axis_config_tready(), .s_axis_data_tdata(cordic_out), .s_axis_data_tvalid(cordic_fin), 
  .s_axis_data_tready(), .s_axis_data_tlast(cordic_out_last), .m_axis_data_tdata(fft_out), .m_axis_data_tvalid(fft_fin), 
  .m_axis_data_tready(1'b1), .m_axis_data_tlast(fft_last), .event_frame_started(), .event_tlast_unexpected(), 
  .event_tlast_missing(), .event_status_channel_halt(), .event_data_in_channel_halt(), 
  .event_data_out_channel_halt());
  
    //get output
    reg  [width-1:0] out_seq_re [0:size-1];
    reg  [width-1:0] out_seq_im [0:size-1];
    reg  [addr_width:0] out_cnt;
    integer j;
    always @(posedge clk) begin
        if(rst) begin
            out_seq_re[0]        <= 'b0;
            out_seq_re[size-1]   <= 'b0;
            out_seq_im[0]        <= 'b0;
            out_seq_im[size-1]   <= 'b0;
            
        end
        else case(state)
            s2:begin
                if(fft_fin == 1'b1) begin
                    out_seq_re[0] <= fft_out[width-1:0];
                    out_seq_im[0] <= fft_out[fft_width-1:width];
                    for(j = size-1;j > 0;j = j-1) begin
                        out_seq_re[j] <= out_seq_re[j-1];
                        out_seq_im[j] <= out_seq_im[j-1];
                    end
                end
            end
            s3:begin
                if((sr_cnt == 'b0) & (out_cnt > 'b0)) begin
                    out_seq_re[0] <= 'b0;
                    out_seq_im[0] <= 'b0;
                    for(j = size-1;j > 0;j = j-1) begin
                        out_seq_re[j] <= out_seq_re[j-1];
                        out_seq_im[j] <= out_seq_im[j-1];
                    end
                end
            end
        endcase
    end

    always @(posedge clk) begin
        if(rst) begin
            out_cnt <= 'b0;
        end
        else case(state)
            s2: out_cnt <= size;
            s3: if(sr_cnt == 'b0) out_cnt <= (out_cnt == 'b0)? 'b0: (out_cnt -1'b1);
            default: out_cnt <= 'b0;
        endcase
    end

    //display output
    reg [width-1:0] out_re_r;
    reg [width-1:0] out_im_r;
    reg out_strb_r;

    assign out_re   = out_re_r;
    assign out_im   = out_im_r;
    assign out_strb = out_strb_r;
    always @(posedge clk) begin
        if(rst) begin
            out_re_r <= 'b0;
            out_im_r <= 'b0;
            out_strb_r <= 1'b0;
        end
        else case(state)
            s3: begin
                out_strb_r <= (sr_cnt == 'b0) & (out_cnt > 'b0);
                out_re_r   <= out_seq_re[size-1];
                out_im_r   <= out_seq_im[size-1];
            end
            default: begin
                out_re_r   <= 'b0;
                out_im_r   <= 'b0;
                out_strb_r <= 1'b0;               
            end
        endcase
    end

    //state machine
    always @(posedge clk) begin
        if(rst) state <= idle;
        else case(state)
            idle: state <= s0;
            s0: state <= (find_r == 1'b1)? s1 : s0;
            s1: state <= s2;
            s2: state <= (fft_last == 1'b1)? s3: s2;
            s3: state <= (out_cnt == 'b0)? s0: s3;
            default: state <= idle;
        endcase
    end
endmodule
